//
//  Path.m
//  SSE
//
//  Created by Matt Roman on 10/29/07.
//  Copyright 2007 __MyCompanyName__. All rights reserved.
//

#import "Path.h"

NSLock *DrawLock;

@interface Path (privatestuff)
// returns NO if the path is clear
// returns YES if there is an intersection
- (BOOL) intersectionStart:(Point3f *)start End:(Point3f *)end CSobj:(Cspace *)CSobj;
// returns the two most extreme points of an obstacle 
// the array contains four values a pair for each point returned describing the type of point
// if the first number is true the second is a vertex on the obstacle else it 
// is an intersection of the sensor arc, if this is true the vertex number of the 
// left most point is -1 and -2 for the right most.
- (NSArray *) extremeOfCspace:(Cspace *)CSobj fromPoint:(Point3f *)startPoint;
- (Point3f) gotoExtreme:(int)obstacleID;
// returns an array of all the extremes of all the visible obstacles
- (NSMutableArray *) visibleExtremesFromPoint:(node *)startNode to:(node *)endNode;
// recursively calculates a clear path from the current rover position to the goal
- (void) clearPath:(node *)startNode to:(node *)endNode;
// calculates the possible paths and finds the shortest path
- (void) NavPathTo:(node *)endNode;
- (void) ShortPath:(node *)endNode;
- (void) obstaclesInRange;
@end

@implementation Path

- (id) initPathWithRover:(Rover *)robo Sense:(float)sense Color:(NSColor *)color
{
	[super init];
	draw = YES;
	pathEff = 0;
	robot = robo;
	onObstacle = -1;
	senseRadius = (sense < 0)?0:sense; 
	Start = [[node alloc] initNodeWithID:-1 point:*[robot getRoverPosition]];
	waypoint = [[node alloc] initNodeWithID:-2 point:*[robot getRoverPosition]];
	End = [[node alloc] initNodeWithID:-2 point:*[robot getGoal]];
	roboPosition = [robot getRoverPosition];
	roboAlpha = [robot getRoverAlpha];
	obstacles = [[NSMutableArray alloc] init];
	Lines = [[NSMutableArray alloc] init];
	DrivenPath = [[NSMutableArray alloc] init];
	ShortestPath = [[NSMutableArray alloc] init];
	NodeList = [[NSMutableArray alloc] init];
	Pcolor = [color retain];
	DrawLock = [[NSLock alloc] init];
	return self;
}

- (void) setPcolor:(NSColor *)color
{
	[color retain];
	[Pcolor release];
	Pcolor = color;
}

- (NSColor *) Pcolor
{
	return Pcolor;
}

- (void) setSharedCspaceArray:(NSMutableArray *)array
{
	CspaceArray = array;
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- (void) testObj:(int)num
{
	int i;
	Point3f here;
	if(0){// test the extremes function
		//here = [[[[CspaceArray objectAtIndex:25] getVertexArray] objectAtIndex:0] getPoint];
		[Lines removeAllObjects];
		here = *roboPosition;
		NSArray *data = [self extremeOfCspace:[CspaceArray objectAtIndex:1] fromPoint:&here];
		if(data==nil) NSLog(@"no data");
		else{
		[Lines addObject:[[[Point3 alloc] initPoint3f:here] autorelease]];
		[Lines addObject:[[[Point3 alloc] initPoint3f:[[data objectAtIndex:1] getPoint]] autorelease]];
			NSLog(@"left Point %f,%f",[[data objectAtIndex:1] getPoint].x,[[data objectAtIndex:1] getPoint].y);
		[Lines addObject:[[[Point3 alloc] initPoint3f:here] autorelease]];
		[Lines addObject:[[[Point3 alloc] initPoint3f:[[data objectAtIndex:3] getPoint]] autorelease]];
			NSLog(@"right Point %f,%f",[[data objectAtIndex:3] getPoint].x,[[data objectAtIndex:3] getPoint].y);
		}
	}
	if(1){// test the visible paths functions
		[self obstaclesInRange];
		[Lines removeAllObjects];
		[Start setPoint:*roboPosition];
		NSMutableArray *tArray = [self visibleExtremesFromPoint:Start to:End];
		for(i=0;i<[tArray count];i++)
		{
			[Lines addObject:[[[Point3 alloc] initPoint3f:[[tArray objectAtIndex:i] getPoint]] autorelease]];
			[Lines addObject:[[[Point3 alloc] initPoint3f:[Start getPoint]] autorelease]];
		}
		[obstacles removeAllObjects];
	}
	if(0){ // test the clear path function
		[End setDistance:nil];
		pathEff = 0;
		[Start setPoint:*roboPosition];
		
		[DrawLock lock];
		[Lines removeAllObjects];
		[ShortestPath removeAllObjects];
		[DrawLock unlock];
		
		[self obstaclesInRange];
		NSLog(@"%d objects in range",[obstacles count]);
		
		[self clearPath:Start to:End];	// calculate the paths between the start point to the end
		[self ShortPath:End];
		NSLog(@"short path %@",ShortestPath);
		[Start setDistance:0];
		[NodeList removeAllObjects];
		[obstacles removeAllObjects];
	}
	 NSLog(@"||||||||||||||||||||||||||||||||");
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// returns YES if there is an intersection otherwise NO
- (BOOL) intersectionStart:(Point3f *)start End:(Point3f *)end CSobj:(Cspace *)CSobj
{
	int j,jplus;
	Point3f tempJunction;
	NSMutableArray *CSvertArray;
	
	CSvertArray = [CSobj getVertexArray];	// get the vertex array from that object
	
	for(j=0; j<[CSvertArray count]; j++) // cycle through all the vertices looking for an intersection
	{
		if(j == [CSvertArray count] - 1) jplus = 0; // if the end of the vertex list is encountered
		else jplus = j+1;							// ensures to check the last vertex with the first
		
		if(SegmentIntersect(start,
							end,
							[[CSvertArray objectAtIndex:j]getPtrPoint],
							[[CSvertArray objectAtIndex:jplus]getPtrPoint],
							&tempJunction))  // checks for intersection of line segments
		{
			//NSLog(@"intersection junction %f %f",tempJunction.x,tempJunction.y);
			// if the vertex being investigated is equal to the starting point continue to the next
			if(!(tempJunction.x == start->x && tempJunction.y == start->y ||
				 tempJunction.x == end->x && tempJunction.y == end->y)) 
			{
				if(senseRadius == 0 || magnitude3d(&tempJunction,roboPosition) < senseRadius-0.001)
				return YES; // return there is an intersection
			}
		}
		
	}
	return NO; // else return NO intersection
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// returns an array containing the most extreme sensed points of an obstacle
- (NSArray *) extremeOfCspace:(Cspace *)CSobj fromPoint:(Point3f *)startPoint
{
	int				k=0;
	int				i,iplus,iminus;
	int				sectType;
	int				CSvertexL,CSvertexR;
	bool			isVertex,isVertexL,isVertexR;
	float			side,lSide,rSide;
	Point3f			currentPoint,previousPoint,nextPoint;
	double			currentDist,vMag;
	Point3f			pLeft;
	Point3f			pRight;
	Point3f			intersect1,intersect2;
	Vector3f		cVect,pVect;
	id				pointL,pointR;
	NSArray			*extremePoints;
	NSMutableArray	*CSvertArray;
	
	CSvertArray = [CSobj getVertexArray];
	i=0;
	isVertex = YES;
	nextPoint.handy = 0;
	side=lSide=rSide=0;

	previousPoint = [[CSvertArray objectAtIndex:i] getPoint];
	
	while(i<[CSvertArray count])
	{
		currentPoint = [[CSvertArray objectAtIndex:i] getPoint];
		
		if(senseRadius > 0)
		{
			if(i == [CSvertArray count] - 1) iplus = 0;
			else iplus = i+1;
		
			nextPoint = [[CSvertArray objectAtIndex:iplus] getPoint];
		
			currentDist = magnitude3d(roboPosition,&currentPoint);
				
			sectType = SegmentIntersectArc(&currentPoint,&nextPoint,roboPosition,senseRadius,&intersect1,&intersect2);
			//NSLog(@"from %d to %d sectType = %d",i,iplus,sectType);
			if(currentDist > senseRadius) // if the first vertex is outside of sensor range 
			{
				isVertex = NO;
				if(sectType == 3) // the second vertex is outside of the sensor range
				{ // but the line segment intersects the arc in two places
					currentPoint = intersect1;
					nextPoint = intersect2;
				}
				else if(sectType == 0) // no intersection both points are out of range
				{	// done looking continue to next vertex
					PntstoVector(startPoint,&previousPoint,&pVect);
					PntstoVector(startPoint,&currentPoint,&cVect);
					side += AngleBetweenUnitVector(&pVect,&cVect);
					previousPoint = currentPoint;
					i++;
					continue;
				}
				else // we have an intersection
				{
					if(sectType == 1) currentPoint = intersect1;
					else currentPoint = intersect2;
					nextPoint.handy = 0;
				}
			}
			else  // if the first vertex is inside the sensor range
			{
				isVertex = YES;
				if(sectType > 0) // we have an intersection
				{
					if(sectType == 1) nextPoint = intersect1;
					else nextPoint = intersect2;
				}
				else nextPoint.handy = 0;
			}
		}
		
		if(k==0)
		{	// if the current point is equal to the start point
			if(currentPoint.x == startPoint->x && currentPoint.y == startPoint->y)
			{
				if(nextPoint.handy)
				{
					currentPoint = nextPoint;
					nextPoint.handy = 0;
				}
				else
				{
					i++;
					continue;
				}
			}
			lSide=rSide=side;
			isVertexL = isVertex;
			isVertexR = isVertex;
			if(isVertex)
			{
				CSvertexL = i;
				CSvertexR = i;	
			}
			else
			{
				CSvertexL = -1;
				CSvertexR = -2;
			}
			pRight = currentPoint;
			pLeft = currentPoint;
			previousPoint = currentPoint;
			k++;
		}
		else
		{
			if(currentPoint.x != startPoint->x && currentPoint.y != startPoint->y)
			{
				PntstoVector(startPoint,&previousPoint,&pVect);
				PntstoVector(startPoint,&currentPoint,&cVect);
				side += AngleBetweenUnitVector(&pVect,&cVect);					
				
				if(side>=lSide)
				{
					lSide = side;
					isVertexL = isVertex;
					CSvertexL = i;
					pLeft = currentPoint;
				}
				else if(side < rSide)
				{
					rSide = side;
					isVertexR = isVertex;
					CSvertexR = i;
					pRight = currentPoint;
				}
				previousPoint = currentPoint;
			}
		}
		
		// if the nextpoint is avaliable 
		if(nextPoint.handy)
		{
			PntstoVector(startPoint,&previousPoint,&pVect);
			PntstoVector(startPoint,&nextPoint,&cVect);
			side += AngleBetweenUnitVector(&pVect,&cVect);
			
			if(side>=lSide)
			{
				lSide = side;
				isVertexL = NO;//isVertex;
				CSvertexL = i;
				pLeft = nextPoint;
			}
			else if(side < rSide)
			{
				rSide = side;
				isVertexR = NO;//isVertex;
				CSvertexR = i;
				pRight = nextPoint;
			}
			previousPoint = nextPoint;		
		}
		i++;
	}
	
	if(k==0) return nil;
	
	if(!isVertexL) CSvertexL = -1;
	else
	{
		iplus = ([CSvertArray count]-1 > CSvertexL)?CSvertexL+1:0;
		iminus = (CSvertexL > 0)?CSvertexL-1:[CSvertArray count]-1;
		PntstoVector([[CSvertArray objectAtIndex:iplus] getPtrPoint],&pLeft,&pVect);
		PntstoVector([[CSvertArray objectAtIndex:iminus] getPtrPoint],&pLeft,&cVect);
		
		pVect.a = pVect.a + cVect.a;
		pVect.b = pVect.b + cVect.b;
		pVect.c = pVect.c + cVect.c;
		
		vMag = magnitude3v(&pVect);
		
		pVect.a /= vMag;
		pVect.b /= vMag;
		pVect.c /= vMag;
		
		pLeft.x += ROVERCSPACE*pVect.a;
		pLeft.y += ROVERCSPACE*pVect.b;
	}
	pointL = [[[Point3 alloc] initPoint3f:pLeft] autorelease];
	
	if(!isVertexR) CSvertexR = -2;
	else
	{
		iplus = ([CSvertArray count]-1 > CSvertexR)?CSvertexR+1:0;
		iminus = (CSvertexR > 0)?CSvertexR-1:[CSvertArray count]-1;
		PntstoVector([[CSvertArray objectAtIndex:iplus] getPtrPoint],&pRight,&pVect);
		PntstoVector([[CSvertArray objectAtIndex:iminus] getPtrPoint],&pRight,&cVect);
		
		pVect.a = pVect.a + cVect.a;
		pVect.b = pVect.b + cVect.b;
		pVect.c = pVect.c + cVect.c;
		
		vMag = magnitude3v(&pVect);
		
		pVect.a /= vMag;
		pVect.b /= vMag;
		pVect.c /= vMag;
		
		pRight.x += ROVERCSPACE*pVect.a;
		pRight.y += ROVERCSPACE*pVect.b;
	}
	pointR = [[[Point3 alloc] initPoint3f:pRight] autorelease];
	extremePoints = [NSArray arrayWithObjects:[NSNumber numberWithInt:CSvertexL],pointL,[NSNumber numberWithInt:CSvertexR],pointR,nil];
		
	return extremePoints;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// returns the extreme point from an obstacle with a spin
- (Point3f) gotoExtreme:(int)obstacleID
{
	NSArray		*pointsArray;
	Cspace		*cspaceObj;
	
	cspaceObj = [CspaceArray objectAtIndex:obstacleID];
	pointsArray = [self extremeOfCspace:cspaceObj fromPoint:roboPosition];
	
	if([cspaceObj spin] == 2) return [[pointsArray objectAtIndex:1] getPoint];
	else return [[pointsArray objectAtIndex:3] getPoint];
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// returns a list of all the nodes visible from the startNode
- (NSMutableArray *) visibleExtremesFromPoint:(node *)startNode to:(node *)endNode
{
	bool			xLeft,xRight;
	int				i,j;
	double			aLeft,aRight;
	double			distanceSoFar;
	Point3f			pLeft,pRight;
	Point3f			*startPoint;
	Cspace			*CspaceObj;
	Vector3f		vGoal;
	Vector3f		vLeft,vRight;
	node			*tNode;
	NSArray			*exeData;
	NSMutableArray	*nextNodeArray;
	
	startPoint = [startNode getPtrPoint];
	PntstoVector(startPoint,[endNode getPtrPoint],&vGoal); // get the unit vector to the goal
	nextNodeArray = [[NSMutableArray alloc] init];
	
	// cycle through all the obstacles in range to find visible extreme limits of obstacles
	for(i=0;i<[obstacles count];i++)
	{
		CspaceObj = [obstacles objectAtIndex:i];
		
		exeData = [self extremeOfCspace:CspaceObj fromPoint:startPoint];
		if(exeData == nil) // the object is not actually in range so we can delete it
		{
			//NSLog(@"object %d not really in range - removed",[[obstacles objectAtIndex:i] getObjectNum]);
			[obstacles removeObjectAtIndex:i];
			i--;
			continue;
		}
		
		pLeft = [[exeData objectAtIndex:1] getPoint];	// get the left extreme point
		PntstoVector(startPoint,&pLeft,&vLeft);			
		aLeft = AngleBetweenUnitVector(&vGoal,&vLeft);	// get the angle to the left extreme based on the vector to the goal
		
		pRight = [[exeData objectAtIndex:3] getPoint];	// get the right extreme point
		PntstoVector(startPoint,&pRight,&vRight);
		aRight = AngleBetweenUnitVector(&vGoal,&vRight);	// get the angle to the right extreme based on the vector to the goal
		
		//NSLog(@"%d object %f,%f   %f,%f",[CspaceObj getObjectNum],pLeft.x,pLeft.y,pRight.x,pRight.y);
		//NSLog(@"%d object angles left %f right %f",[CspaceObj getObjectNum],RADTODEG(aLeft),RADTODEG(aRight)); NSLog(@" ");
		//if(aLeft > 0 && aRight < 0) clearToGoal = NO;		// if the left angle is positive and the right is negative
		// there is an obstacle in front of the robot
		
		xLeft = NO;
		xRight = NO;
		// cycle through other obstacles to make sure they are not in the way
		for(j=0;j<[obstacles count];j++)	
		{
			if(j == i) continue; // dont look at the same object for intersections
			// is there an intersection between the current CspaceObj extremes and any other objects in range?
			if([self intersectionStart:startPoint End:&pLeft CSobj:[obstacles objectAtIndex:j]]) xLeft = YES;
			if([self intersectionStart:startPoint End:&pRight CSobj:[obstacles objectAtIndex:j]]) xRight = YES;
		}
		
		if([CspaceObj spin] == 1) xLeft = YES;
		else if([CspaceObj spin] == 2) xRight = YES;
		
		if(!xLeft)	// vertex is clear and can be driven to then add a node to the nextNodeArray
		{	
			// calculate the distance so far to the vertex
			distanceSoFar = [startNode distance] + magnitude3d(startPoint,&pLeft);
			//NSLog(@"saved left point %f,%f",pLeft.x,pLeft.y);
			// create a temp node to be placed in the temp array of nodes that can be driven to next
			tNode = [[node alloc] initNodeWithID:[CspaceObj getObjectNum] point:pLeft];
			[tNode setVertexNumber:[[exeData objectAtIndex:0] intValue]];
			[tNode setDistance:distanceSoFar];
			[tNode setDirection:fabs(aLeft)];
			[tNode setParentNode:startNode];
			[nextNodeArray addObject:tNode];
			[tNode release];
			
			//[Lines addObject:[[[Point3 alloc] initPoint3f:*startPoint] autorelease]];
			//[Lines addObject:[[[Point3 alloc] initPoint3f:pLeft] autorelease]];
		}
		
		if(!xRight) //vertex is clear and can be driven to add a node to the nextNodeArray
		{
			distanceSoFar = [startNode distance] + magnitude3d(startPoint,&pRight);
			//NSLog(@"saved right point %f,%f",pRight.x,pRight.y);
			// create a temp node to be placed in the temp array of nodes that can be driven to next
			tNode = [[node alloc] initNodeWithID:[CspaceObj getObjectNum] point:pRight];
			[tNode setVertexNumber:[[exeData objectAtIndex:2] intValue]];
			[tNode setDistance:distanceSoFar];
			[tNode setDirection:fabs(aRight)];
			[tNode setParentNode:startNode];
			[nextNodeArray addObject:tNode];
			[tNode release];
			
			//[Lines addObject:[[[Point3 alloc] initPoint3f:*startPoint] autorelease]];
			//[Lines addObject:[[[Point3 alloc] initPoint3f:pRight] autorelease]];
		}
	}
	[nextNodeArray sortUsingSelector:@selector(compareAngle:)];
	
	return nextNodeArray;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// captures all the obstacles within range of the sensor
- (void) obstaclesInRange
{
	int i;
	Cspace *CspaceObj;
	
	for(i=0;i<[CspaceArray count];i++)	// cycle through all the Cspace obstacles to find the ones in range
	{
		CspaceObj = [CspaceArray objectAtIndex:i]; // get a Cspace object
		// check to verify the object is in range
		if(senseRadius == 0 || senseRadius + [CspaceObj getRadius] > magnitude3d(roboPosition,[CspaceObj getCenter])) 			
		{
			// if it is in range add it to the obstacles list
			[obstacles addObject:CspaceObj];
			//NSLog(@"%d obstacle added",i);
		}
	}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// finds a path from startLeaf to the goal using the obstacles array
- (void) clearPath:(node *)startNode to:(node *)endNode
{
	bool			clearToGoal = YES;
	bool			inNodeArray,inNodeList;
	int				i,j,k;
	Point3f			*startPoint;
	node			*tNode;
	node			*nextNode;
	NSMutableArray	*tempNodeArray;
	NSMutableArray  *nextNodeArray;
	
	startPoint = [startNode getPtrPoint];
	[startNode setVisited:YES];
	
	//NSLog(@"start from %f,%f",startPoint->x,startPoint->y);
	//NSLog(@"current obstacle %d vertex %d",[startNode getID],[startNode vertexNumber]);
	// cycle through obstacles to check for clear goal path
	if(senseRadius == 0 || magnitude3d(startPoint,roboPosition) < senseRadius-0.001)
	{
		for(j=0;j<[obstacles count];j++)	
		{
			// is there an intersection between the current CspaceObj extremes and any other objects in range?
			if([self intersectionStart:startPoint End:[endNode getPtrPoint] CSobj:[obstacles objectAtIndex:j]])
			{
				clearToGoal = NO;
				break;
			}
		}
	}
	
	if(clearToGoal) // if the path is clear to the goal
	{
		tempDist = [startNode distance] + magnitude3d(startPoint,[endNode getPtrPoint]);
		if(tempDist < [endNode distance] || [endNode distance] == nil) // if the distance is less than the current shortest distance
		{
			[endNode setDistance:tempDist];   // update the distance
			[endNode setParentNode:startNode]; // reset the parent node
		}
		
		[DrawLock lock]; // lock the following array because it is used in a seperate thread for drawing
		[Lines addObject:[[[Point3 alloc] initPoint3f:*startPoint] autorelease]];
		[Lines addObject:[[[Point3 alloc] initPoint3f:[endNode getPoint]] autorelease]];
		[DrawLock unlock];
		
	}
	else if(senseRadius == 0 || magnitude3d(startPoint,roboPosition) < senseRadius-0.001) // is the start point on the edge of the sensor radius
	{
		// nextNodeArray contains all the possible pathways from the startNode
		nextNodeArray = [self visibleExtremesFromPoint:startNode to:endNode];
		//NSLog(@"Nodes available %@",nextNodeArray);
		// check for duplicates or shorter paths in the global NodeList array
		for(i=0;i<[nextNodeArray count];i++)
		{
			nextNode = [nextNodeArray objectAtIndex:i];
			inNodeList = NO;
			for(j=0;j<[NodeList count];j++)
			{
				tempNodeArray = [NodeList objectAtIndex:j];
				
				if([[tempNodeArray objectAtIndex:0] getID] == [nextNode getID])
				{
					inNodeList = YES;
					break;
				}
				if([[tempNodeArray objectAtIndex:0] getID] > [nextNode getID]) break; 
			}
			
			if(inNodeList) // if an array for the specified Cspace ID is already created check the list for the current nextNode
			{
				inNodeArray = NO;
				for(k=0;k<[tempNodeArray count];k++)
				{
					tNode = [tempNodeArray objectAtIndex:k];
					if([tNode vertexNumber] == [nextNode vertexNumber])
					{
						inNodeArray = YES;
						break;
					}
				}
				
				if(inNodeArray)// if the current nextNode is already in the array
				{
					//if the node saved in the global array is farther away than the current nextNode
					//remove the old saved node and replace it with the current nextNode
					if([tNode distance] > [nextNode distance])
					{
						[tNode setVisited:NO];
						[tNode setDistance:[nextNode distance]];
						[tNode setDirection:[nextNode direction]];
						[tNode setParentNode:[nextNode parentNode]];
					}

					// remove the current nextNode if it has already been visited so we don't go there again
					if([tNode visited])
					{
						[nextNodeArray removeObjectAtIndex:i];
						i--;
					}
					else
					{
						[nextNodeArray removeObjectAtIndex:i];
						[nextNodeArray insertObject:tNode atIndex:i];
					}
				}
				else // add the current nextNode to the current nodeArray
				{
					[tempNodeArray addObject:nextNode];
				}
			}
			else  // add a new array to the NodeList with the object ID
			{
				NSMutableArray *ta = [[NSMutableArray alloc] init];
				[ta addObject:nextNode];
				[NodeList insertObject:ta atIndex:j];
				[ta release];
			}
		}
		/*
		if(1)
		{
			for(i=0;i<[nextNodeArray count];i++)
			{
				NSLog(@"next object %d and vertex %d direction %f",[[nextNodeArray objectAtIndex:i] getID],[[nextNodeArray objectAtIndex:i] vertexNumber],[[nextNodeArray objectAtIndex:i] direction]);
			}
			//NSLog(@"not clear %d vertices detected",[nextNodeArray count]);
			//NSLog(@"Node List contains %d obstacles",[NodeList count]);
			NSLog(@"*************************************");
			/*for(i=0;i<[NodeList count];i++)
			{
				NSLog(@"Obstacle %d has %d visible nodes",[[[NodeList objectAtIndex:i] objectAtIndex:0] getID],[[NodeList objectAtIndex:i] count]);
				for(j=0;j<[[NodeList objectAtIndex:i] count];j++)
				{
					NSLog(@"   vertex %d visited? %d",[[[NodeList objectAtIndex:i] objectAtIndex:j] vertexNumber],[[[NodeList objectAtIndex:i] objectAtIndex:j] visited]);
				}
			}
			NSLog(@"--------------------------------------");
		}*/
		//NSLog(@"Node list %@",NodeList);
		for(i=0;i<[nextNodeArray count]; i++)
		{
			nextNode = [nextNodeArray objectAtIndex:i];
			//NSLog(@"next node %@",nextNode);
			if(i >= pathBreadthPerPass) break;

				[DrawLock lock];
				[Lines addObject:[[[Point3 alloc] initPoint3f:*startPoint] autorelease]];
				[Lines addObject:[[[Point3 alloc] initPoint3f:[[nextNodeArray objectAtIndex:i] getPoint]] autorelease]];
				[DrawLock unlock];

				//NSLog(@"next obstacle %d vertex %d direction %f",[nextNode getID],[nextNode vertexNumber],[nextNode direction]);
			[self clearPath:nextNode to:endNode];
		}
	}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// initializes the shortest path variables and calulates the avaliable paths
- (void) NavPathTo:(node *)endNode
{
	int i;
	[endNode setDistance:nil];
	pathEff = 0;
	[Start setPoint:*roboPosition];
	[Start setDistance:0];
	
	[DrawLock lock];
	[Lines removeAllObjects];
	[ShortestPath removeAllObjects];
	if(driveComplete){
		for(i=0;i<[CspaceArray count];i++)
		{
			[[CspaceArray objectAtIndex:i] setSpin:0];
			[[CspaceArray objectAtIndex:i] setSpinLock:NO];
		}
		[DrivenPath removeAllObjects];
		driveComplete = NO;
	}
	[DrawLock unlock];
	
	[self obstaclesInRange]; // finds all obstacles within range
	//NSLog(@"%d objects in range",[obstacles count]);
	
	[self clearPath:Start to:endNode];	// calculate the paths between the start and end nodes
	//NSLog(@"node list %@",NodeList);
	[self ShortPath:endNode];
	//NSLog(@"short path %@",ShortestPath);
	shortDist = [endNode distance];
	[NodeList removeAllObjects];
	[obstacles removeAllObjects];
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Find the shortest path in the path tree
- (void) ShortPath:(node *)endNode
{
	if([endNode getID] != [Start getID]){		// recursively move to the start of the path
		[self ShortPath:[endNode parentNode]];
	}
	[ShortestPath addObject:endNode];
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// called within the drawing loop of OpenGL stuff
- (void) DrawPathways:(BOOL)x
{
	int i,link;
	Point3f p1,p2;

	if([DrawLock tryLock]){
		if(x && Lines != nil){// draws all the paths avaliable
		
			link = [Lines count];

			if(link % 2){
				link--;
			}
			
			glColor4f(0.0,0.0,0.4,1);
			glBegin(GL_LINES);
				for(i=0;i<link;i+=2)
				{
					if(i/2 >= pathBreadthPerPass) glColor4f(0.0,0.0,0.4,1);
					else glColor4f(0.5,0.0,0.0,1);
					//NSLog(@"%d -----------------------------",i);
					p1 = [[Lines objectAtIndex:i]getPoint];
					p2 = [[Lines objectAtIndex:i+1]getPoint];
					glVertex3f(p1.x,p1.y,p1.z);
					glVertex3f(p2.x,p2.y,p2.z);
				}
			glEnd();
		}
		[DrawLock unlock];
	}

	if(End != nil && draw){// draws the shortest/driven path
		glColor4f([Pcolor redComponent],[Pcolor greenComponent],[Pcolor blueComponent],[Pcolor alphaComponent]);
		glBegin(GL_LINE_STRIP);
		for(i=0;i<[ShortestPath count];i++)
		{
			p1 = [[ShortestPath objectAtIndex:i] getPoint]; // pulls the vertices
			glVertex3f(p1.x,p1.y,p1.z+0.05);
		}
		glEnd();
		glColor4f([Pcolor redComponent],[Pcolor greenComponent],[Pcolor blueComponent],[Pcolor alphaComponent]);
		glBegin(GL_LINE_STRIP);
		for(i=0;i<[DrivenPath count];i++)
		{
			p1 = [[DrivenPath objectAtIndex:i] getPoint];
			glVertex3f(p1.x,p1.y,p1.z+0.06);
		}
		glEnd();
	}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- (BOOL) DrivePath
{
	int				i;
	int				currentSpin,spinDirection;
	static int		borderObst=-1;		// obstacle # the rover is nearest
	static int		swap = 0;
	static float	nextAlpha;
	static float	prevAlpha;
	static float	tempAlpha = 0;
	static float	releaseAngle;
	static int		turns = 0;
	Vector3f		toVector,fromVector,outVector;
	Vector3f		spinVector,east;
	Point3f			tempPoint;
	Point3f			tempRoboPosition;
	double			distNext;
	double			offset;
	double			spread;
	
	if(senseRadius == 0){
		[self NavPathTo:End];	// find the shortest path to the goal from where the rover is located
		roboTurn = [ShortestPath count]-2; // subtract the start point and end point
		pathEff = magnitude3d([[ShortestPath objectAtIndex:0] getPtrPoint],[[ShortestPath lastObject] getPtrPoint])/[End distance];
		return YES;	// if we are calculating the Gods Eye path we are done
	}
	
	east.a = 1.0;
	east.b = 0.0;
	east.c = 0.0;
	
	tempRoboPosition = *roboPosition;
	fromVector = *[robot getHeading];
	
	if(onObstacle == -1) {
		[self NavPathTo:End];	// find the shortest path to the goal from where the rover is located
		// find the direction to the next point on the path
		distNext = PntstoVector(&tempRoboPosition,[[ShortestPath objectAtIndex:1] getPtrPoint],&toVector); 
	}
	else
	{
		[waypoint setPoint:[self gotoExtreme:onObstacle]];
		[self NavPathTo:waypoint];
		// find the direction to the next point on the path
		distNext = PntstoVector(&tempRoboPosition,[[ShortestPath objectAtIndex:1] getPtrPoint],&toVector); 
		releaseAngle += AngleBetweenUnitVector(&fromVector,&toVector);
		//NSLog(@"release angle %f goal angle %f",RADTODEG(releaseAngle),RADTODEG(AngleBetweenUnitVector(&east,[robot getGoalVector])));
		if([[CspaceArray objectAtIndex:onObstacle] spin] == 2) // if CW around obstacle
		{
			if(releaseAngle <= AngleBetweenUnitVector(&east,[robot getGoalVector]))
			{
				//NSLog(@"CW released");
				onObstacle = -1;
				[waypoint clear];
				[self NavPathTo:End];
				// find the direction to the next point on the path
				distNext = PntstoVector(&tempRoboPosition,[[ShortestPath objectAtIndex:1] getPtrPoint],&toVector); 
			}
		}
		else // else if going CCW around obstacle
		{
			if(releaseAngle >= AngleBetweenUnitVector(&east,[robot getGoalVector]))
			{
				//NSLog(@"CCW released");
				onObstacle = -1;
				[waypoint clear];
				[self NavPathTo:End];
				// find the direction to the next point on the path
				distNext = PntstoVector(&tempRoboPosition,[[ShortestPath objectAtIndex:1] getPtrPoint],&toVector); 
			}
		}
	}
	
	

	// point the rover in the correct direction
	prevAlpha = nextAlpha;
	nextAlpha = fabs(atan(toVector.b/toVector.a));
	if(toVector.a < 0) nextAlpha = PI - nextAlpha;
	if(toVector.b < 0) nextAlpha = TWOPI - nextAlpha;
	
	if(fabs(tempAlpha - *roboAlpha) > 0.008)
	{ // if the change in alpha is greater than about 1 deg. count it as a turn 
		turns++;
		tempAlpha = *roboAlpha;
	}
	[DrivenPath addObject:[[[Point3 alloc] initPoint3f:*roboPosition] autorelease]];	// otherwise save the current location of the rover
	
	//exit condition
	if(magnitude3d(&tempRoboPosition,[End getPtrPoint]) <= senseRadius){	// if the distance to the goal is within the sensor range 
		turns += [ShortestPath count] - 1;
		[ShortestPath removeObjectAtIndex:0]; // remove the rovers position because it was already added to the drive path
		for(i=0;i<[ShortestPath count];i++)
		{
			[DrivenPath addObject:[[[Point3 alloc] initPoint3f:[[ShortestPath objectAtIndex:i] getPoint]] autorelease]];
		}
		
		roboTurn = turns-2;
		turns = 0;
		tempAlpha = 0;
		borderObst=-1;
		onObstacle=-1;
		swap=0;
		i = 0;
		shortDist = 0;
		// add up the total path length
		while(i < [DrivenPath count]-1)
		{
			shortDist += magnitude3d([[DrivenPath objectAtIndex:i] getPtrPoint],[[DrivenPath objectAtIndex:i++] getPtrPoint]);
		}
		pathEff = magnitude3d([[DrivenPath objectAtIndex:0] getPtrPoint],[[DrivenPath lastObject] getPtrPoint])/shortDist;
		incompletePath = NO;
		driveComplete = YES;
		return YES;
	} 
	else{	// drive toward the first point on the calculated path
		if(distNext < pathStep) 
		{
			tempRoboPosition = *[[ShortestPath objectAtIndex:1] getPtrPoint];
		}
		else
		{
			tempRoboPosition.x += pathStep*toVector.a;
			tempRoboPosition.y += pathStep*toVector.b;
			if(borderObst == [[ShortestPath objectAtIndex:1] getID] && borderObst != [End getID])
			{
				tempPoint = *[[CspaceArray objectAtIndex:borderObst] getCenter];
				offset = ROVERCSPACE+PntstoVector(&tempRoboPosition,&tempPoint,&outVector);
				tempRoboPosition.x = tempPoint.x - offset*outVector.a;
				tempRoboPosition.y = tempPoint.y - offset*outVector.b;
			}
			else borderObst = [[ShortestPath objectAtIndex:1] getID];
		}
		
		if(borderObst != [End getID])
		{
			spinDirection = NormalizeCross([robot getGoalVector],&toVector,&spinVector);
			spread =  AngleBetweenUnitVector([robot getGoalVector],&toVector) - AngleBetweenUnitVector([robot getGoalVector],&fromVector);
			spread = fabs(spread);
			if(spread > DEGTORAD(140))
			{
				swap++;
				currentSpin = [[CspaceArray objectAtIndex:borderObst] spin];
				if(currentSpin == 0){
					if(spinDirection > 0) currentSpin = 2;
					else currentSpin = 1;
					[[CspaceArray objectAtIndex:borderObst] setSpin:currentSpin];
					//NSLog(@"spinDir %d spin %@",currentSpin,(currentSpin == 2)?@"CW":@"CCW");
				}
				else if([[CspaceArray objectAtIndex:borderObst] spinLock] == NO)
				{
					[[CspaceArray objectAtIndex:borderObst] setSpin:(currentSpin == 2)?1:2];
					[[CspaceArray objectAtIndex:borderObst] setSpinLock:YES];
					NSLog(@"swapped direction");
				}
				
				releaseAngle = AngleBetweenUnitVector(&east,&toVector);
				onObstacle = borderObst;
				//NSLog(@"release angle %f on obstacle %d",RADTODEG(releaseAngle),onObstacle);
			}
		}
		
		*roboPosition = tempRoboPosition;
		*roboAlpha = nextAlpha;	// point the rover in the correct direction
		
	}
	if(swap > 20) {
		swap=0;
		onObstacle=-1;
		turns=0;
		NSLog(@"Incomplete path");
		incompletePath = YES;
		driveComplete = YES;
		return YES;
	}
	return NO;
}

- (float) pathDistance
{
	return shortDist;
}

- (float) senseRadius
{
	return senseRadius;
}

- (void) setSenseRadius:(float)sr
{
	senseRadius = (sr < 0)?0:sr;
}

- (int) roboTurn
{
	return roboTurn;
}

- (NSNumber *) pathDisplay
{
	return [NSNumber numberWithBool:draw];
}

- (double) pathEfficiency
{
	return pathEff;
}

- (void) setPathDisplay:(NSNumber *)flag
{	
	draw = ([flag intValue])?YES:NO;
}

- (void) setPathBreadthPerPass:(int)x
{
	pathBreadthPerPass = (x < 1)?1:x;
}

- (void) setAngularLimit:(float)a
{
	pathAngleLimit = (a < 1)?1:a;
}

- (void) setPathStep:(float)step
{
	pathStep = step;
}

- (bool) incompletePath
{
	return incompletePath;
}
- (void) dealloc
{
	roboPosition = nil;
	[Pcolor release];
	[Start release];
	[waypoint release];
	[End release];
	CspaceArray = nil;
	[ShortestPath release];
	[DrivenPath release];
	[obstacles release];
	[NodeList release];
	[super dealloc];
}
@end
